﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;

namespace LowLevelGraphics.Filter
{
    /// <summary>
    /// 
    /// </summary>
    public enum DEPTH_MAP_MODE
    {
        /// <summary>
        /// This is the default mode that will return a default depthmap
        /// </summary>
        DEFAULT,
        /// <summary>
        /// This will take an approach similar to the 3rd one, but much simplified.
        /// </summary>
        ANAGLYPH,
        /// <summary>
        /// This should get us a really good depth map, including stereo discontinuities.
        /// </summary>
        PIXEL_BY_PIXEL_STEREO 
    }

    /// <summary>
    /// This needs two pics as an input
    /// </summary>
    public class DepthMap : BaseImageFilter, ITwoSourceFilter
    {
        protected DEPTH_MAP_MODE m_DEPTH_MAP_MODE = DEPTH_MAP_MODE.ANAGLYPH;

        /// <summary>
        /// Initializes a new instance of the <see cref="DepthMap"/> class.
        /// </summary>
        public DepthMap()
        {
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="DepthMap"/> class.
        /// </summary>
        /// <param name="_depthMap">The _depth map.</param>
        internal DepthMap(DepthMap _depthMap)
        { 
        }

        /// <summary>
        /// Execute this Filter
        /// A Debug Statement is written if a concrete Filter is called
        /// </summary>
        /// <param name="_bitmap"></param>
        /// <returns></returns>
        public override UnsafeBitmap Execute(UnsafeBitmap _bitmap)
        {
            return Process((UnsafeBitmap)_bitmap.Clone(), _bitmap);
        }

        #region ITwoSourceFilter Members

        /// <summary>
        /// Processes the specified bmpimg.
        /// </summary>
        /// <param name="_bitmap1"></param>
        /// <param name="_bitmap2"></param>
        /// <returns></returns>
        public UnsafeBitmap Process(UnsafeBitmap _bitmap1, UnsafeBitmap _bitmap2)
        {
            UnsafeBitmap depthMap = null;
            switch (m_DEPTH_MAP_MODE)
            {
                case DEPTH_MAP_MODE.DEFAULT:
                    //Create Depthmap directly from stereo pairs
                    depthMap = Method1(_bitmap1, _bitmap2);
                    break;

                case DEPTH_MAP_MODE.ANAGLYPH:
                    depthMap = Method2(_bitmap1, _bitmap2);
                    break;

                case DEPTH_MAP_MODE.PIXEL_BY_PIXEL_STEREO:
                    depthMap = Method3(_bitmap1, _bitmap2);
                    break;
            }


            return depthMap;
        }

        /// <summary>
        /// 
        /// </summary>
        public struct DepthmapParams
        {
            public int radius;
            public int parallax;

            /// <summary>
            /// Initializes a new instance of the <see cref="DepthmapParams"/> struct.
            /// </summary>
            /// <param name="_radius">The _radius.</param>
            /// <param name="_parallax">The _parallax.</param>
            public DepthmapParams(int _radius, int _parallax)
            {
                radius = _radius;
                parallax = _parallax;
            }
        }

        /// <summary>
        /// Coords the specified x.
        /// </summary>
        /// <param name="x">The x.</param>
        /// <param name="y">The y.</param>
        /// <param name="k">The k.</param>
        /// <param name="channels">The channels.</param>
        /// <param name="width">The width.</param>
        /// <returns></returns>
        protected static int coord(int x, int y, int k, int channels, int width)
        {
            return channels * (width * y + x) + k;
        }

        /// <summary>
        /// The two layers need to be identical in size.
        /// The algorithm takes a pixel of the first layer plus its surrounding area
        /// and searches on the second layer for the best match.
        /// First option changes the horizontal parallax size the algorithm searches for a match.
        /// Try smaller values first, it's getting slow with big ones.
        /// Second option is the radius of the tile, which is compared to the tiles
        /// of the other image in the search area. Start here with small values too.
        /// This algorithm can't interpret the image content,
        /// so the results will be 'ugly' in digital noisy or under- or overexposed areas.
        /// </summary>
        /// <param name="_bitmapLeft">The _bitmap left.</param>
        /// <param name="_bitmapRight">The _bitmap right.</param>
        /// <returns></returns>
        private static UnsafeBitmap Method2(UnsafeBitmap _bitmapLeft, UnsafeBitmap _bitmapRight)
        {
            DepthmapParams depthmap_params = new DepthmapParams(2, 5);
            UnsafeBitmap bitmapResult = (UnsafeBitmap)_bitmapLeft.Clone();
            int width = _bitmapLeft.Width;
            int height = _bitmapLeft.Height;
            int parallax = depthmap_params.parallax;
            int radius = depthmap_params.radius;

            //Algorithm begins here:
            int x;
            int y;
            int i;
            int diff;
            int diffnew;
            int k;
            int xk;
            int yk;
            int besthit;
            int channels = _bitmapLeft.BytesPerPixelCount;
            int[] rectleft = new int[width * height * 4];
            int[] rectright = new int[width * height * 4];
            int[] rectdepth = new int[width * height * 4];

            for (y = radius; y < height - radius; y++)
            {
                for (x = parallax + radius; x < (width - radius); x++)
                {
                    diff = 10000000;
                    diffnew = 0;
                    besthit = 0;
                    //search at the left side for the best fit
                    for (i = 0; i > -parallax; i--)
                    {
                        diffnew = 0;
                        for (k = 0; k < channels; k++)
                        {
                            for (xk = -radius; xk <= radius; xk++)
                            {
                                for (yk = -radius; yk <= radius; yk++)
                                {
                                    diffnew += Math.Abs(
                                        (int)rectright[
                                            coord(x + i + xk, y + yk, k, channels, width)]
                                            -
                                        (int)rectleft[
                                            coord(x + xk, y + yk, k, channels, width)]);
                                }
                            }
                        }
                        //set besthit, if the new diff is lower than old diff
                        if (diffnew < diff)
                        {
                            besthit = -i;
                            diff = diffnew;
                        }
                    }
                    //set depth
                    for (i = 0; i < (channels - 1); i++)
                    {
                        rectdepth[coord(x, y, i, channels, width)] = besthit;
                    }
                    rectdepth[coord(x, y, (channels - 1), channels, width)] = 255;
                }
            }

            int nIndex = 0;
            for (y = 0; y < height; y++)
            {
                for (x = 0; x < width; x++)
                {
                    bitmapResult.SetGrayPixel(x, y, rectdepth[nIndex]);
                    nIndex++;
                }
            }
            return bitmapResult;
        }

        /// <summary>
        /// Method1s the specified _bitmap1.
        /// </summary>
        /// <param name="_bitmap1">The _bitmap1.</param>
        /// <param name="_bitmap2">The _bitmap2.</param>
        /// <returns></returns>
        private static UnsafeBitmap Method1(UnsafeBitmap _bitmap1, UnsafeBitmap _bitmap2)
        {
            int col = 0;
            int bestCol2 = 0;
            int col2 = 0;
            UnsafeBitmap otherImage = _bitmap2;
            UnsafeBitmap depthMap = _bitmap1;
            depthMap.Clear(Color.Black);

            int y = 0;
            int x = 0;
            int nWidth = 0;
            int nHeight = 0;

            int rowStart = 0;
            int nIncrement = 3;
            int nOffset = 6;
            int rowOffset = 0;
            int colOffset = 0;
            int row2 = 0;
            int i = 0;
            int[,] tempIntensity = new int[20, 20];
            long intensityMatch = 0;
            int bestMatch = 0;

            for (y = 0; y < nHeight; y += nIncrement)
            {
                for (x = 0; x < nWidth; x += nIncrement)
                {
                    for (rowOffset = 0; rowOffset < nOffset; rowOffset++)
                    {
                        for (colOffset = 0; colOffset < nOffset; colOffset++)
                        {
                            i = ((y + rowOffset) * 3 * nWidth + 3 * (x + colOffset));
                            tempIntensity[rowOffset, colOffset] = ColorFunctions.ToIntRGB(otherImage[y + rowOffset, x + colOffset]);
                        }
                    }
                    // Decide where to start
                    if (y < nHeight - 5)
                    {
                        row2 = y + 5;
                    }
                    else
                    {
                        row2 = nHeight - y;
                    }

                    bestCol2 = 0;
                    bestMatch = 9999999;
                    col2 = 0;
                    // Look for a match
                    while (row2 > rowStart - 5)
                    {
                        while (col2 < nWidth - nOffset)
                        {
                            intensityMatch = 0;
                            for (rowOffset = 0; rowOffset < nOffset; rowOffset++)
                            {
                                for (colOffset = 0; colOffset < nOffset; colOffset++)
                                {
                                    i = ((row2 + rowOffset) * 3 * nWidth + 3 * (col2 + colOffset));
                                    intensityMatch += Math.Abs(tempIntensity[rowOffset, colOffset] - ColorFunctions.ToIntRGB(depthMap.GetPixel(depthMap.GetCoordinatesFromIndex(i))));
                                }
                            }
                            // If the current match is better than the other matches, save it
                            if (intensityMatch < bestMatch)
                            {
                                bestMatch = (int)intensityMatch;
                                bestCol2 = col2;
                            }
                            col2++;
                        }
                        row2--;
                    }

                    // Set depth map value if it is a possible real depth
                    if (x - bestCol2 > 0 && x - bestCol2 < 127 && bestMatch < 5000)
                    {
                        depthMap[x, y] = Color.FromArgb(2 * Math.Abs(x - bestCol2));
                    }
                }
            }
            return depthMap;
        }

        /// <summary>
        /// Method3s the specified _bitmap1.
        /// </summary>
        /// <param name="_bitmap1">The _bitmap1.</param>
        /// <param name="_bitmap2">The _bitmap2.</param>
        /// <returns></returns>
        private static UnsafeBitmap Method3(UnsafeBitmap _bitmap1, UnsafeBitmap _bitmap2)
        {
            return null;
        }
        #endregion

        /// <summary>
        /// This is used to clone a filter, each filter must implement it's own clone functionality
        /// </summary>
        /// <returns></returns>
        public override object Clone()
        {
            return new DepthMap(this);
        }
    }
}
